#ifndef __CONTROL_H
#define __CONTROL_H

#include "bsp_timer.h"
#include "app_include.h"
#include "spl06.h"
#include <math.h>


#define TARGET_TEMERATURE 65.f
#define HEATING_CONTROL_KP 2.5f		//2.5f
#define HEATING_CONTROL_KI 0.6f		//5.0f
#define HEATING_CONTROL_KD 0.5f		//3.5f

typedef struct
{
	float K;
	float w1;
	float w2;
	float w3;
	float last_w1;
	float last_w2;
	float last_w3;
	/* input weight */
	
	float np;
	float ni;
	float nd;
	/* learning rate */
		
	float error[3];
	/*last three errors*/
	
	float x[3];
	/* input vector */
	
	float output;
	float last_output;
	float delta_output;
	float output_limit;
	/* increasment output */
}neurons_pid_t;



typedef struct
{
	float kp;
	float ki;
	float kd;
	
	float error[3];
	
	float output;
	float output_limit;
}incremental_pid_t;


float neurons_pid(neurons_pid_t *pid,float error);
void incremental_pid(incremental_pid_t *pid,float error);


float motorspeed_control(void);
float Height_Control(void *arg);
void Height_Controler_Reset(void);


void Heating_Control(void);
void Servo_AngleSolution(float *angle,float *m);
void Attitude_Control(void *arg);
void Servo_AngleControl(float *angle);
void PID_Calculate(void *arg);


void Position_Control(void *arg);
float servo_resolve(float torque,float wm);
	

#endif


